﻿using System;
using System.Linq;
using System.Collections.Generic;
using System.Text;
using System.Drawing;
using System.Diagnostics;
using MLRobotic.Tower.Common;

namespace MLRobotic.Tower.Detection
{
    public static class HsvHelper
    {
        private static Log _log = new Log("HsvHelper");

        // http://en.wikipedia.org/wiki/HSL_and_HSV
        public static void ColorToHSV(Color color, out double hue, out double saturation, out double value)
        {
            double r = color.R / 255d;
            double g = color.G / 255d;
            double b = color.B / 255d;

            double max = Math.Max(r, Math.Max(g, b));
            double min = Math.Min(r, Math.Min(g, b));
            double chroma = max - min;

            hue = 0;

            if (chroma == 0)         
                hue = 0;
            else if (max == r)
                hue = ((g - b) / chroma) % 6;
            else if (max == g)
                hue = ((b - r) / chroma) + 2;
            else if (max == b)
                hue = ((r - g) / chroma) + 4;

            hue *= 60;

            saturation = (chroma == 0) ? 0 : chroma / max;
            value = max;
        }

        public static void ColorToHSL(Color color, out double hue, out double saturation, out double brightness)
        {
            ColorToHSL(color.R, color.G, color.B, out hue, out saturation, out brightness);
        }

        public static void ColorToHSL(byte r, byte g, byte b, out double hue, out double saturation, out double brightness)
        {
            double max = Math.Max(r, Math.Max(g, b));
            double min = Math.Min(r, Math.Min(g, b));
            double chroma = max - min;

            hue = 0d;
            saturation = 0d;

            if (chroma == 0)
                hue = 0;
            else if (max == r)
                hue = ((g - b) / chroma); // %6 for -180:180 range
            else if (max == g)
                hue = ((b - r) / chroma) + 2;
            else if (max == b)
                hue = ((r - g) / chroma) + 4;

            hue *= 60;

            if (hue < 0)
                hue += 360;

            if (max != min)
                saturation = (max + min) > 255d ? chroma / (510d - max - min) : chroma / (max + min);

            brightness = (max + min) / 510d;
        }

        public static void Color565ToHSL(byte low, byte hight, out double hue, out double saturation, out double brightness)
        {
            double r = ((double)(hight >> 3)) / 31;
            double g = ((double)(((hight << 3) & 0x38) | (low >> 5))) / 63;
            double b = ((double)(low & 0x1F)) / 31;

            double max = Math.Max(r, Math.Max(g, b));
            double min = Math.Min(r, Math.Min(g, b));
            double chroma = max - min;

            hue = 0d;
            saturation = 0d;

            if (chroma == 0)
                hue = 0;
            else if (max == r)
                hue = ((g - b) / chroma); // %6 for -180:180 range
            else if (max == g)
                hue = ((b - r) / chroma) + 2;
            else if (max == b)
                hue = ((r - g) / chroma) + 4;

            hue *= 60;

            if (hue < 0)
                hue += 360;

            if (max != min)
                saturation = (max + min) > 1 ? chroma / (2 - max - min) : chroma / (max + min);

            brightness = (max + min) / 2;
        }


        private static double[,] _map = null;

        public static void MapColor565ToHSL(byte low, byte hight, out double hue, out double saturation, out double brightness)
        {
            if (_map == null)
                _map = CreateMap();

            var index = low + hight * 256;

            hue = _map[index, 0];
            saturation = _map[index, 1];
            brightness = _map[index, 2];
        }

        private static double[,] CreateMap()
        {
            _log.DebugStartWatch("CreateMap");

            int count = 65536;
            double h;
            double s;
            double v;

            var map = new double[count, 3];

            for (int low = 0; low < 256; low++)
            {
                for (int high = 0; high < 256; high++)
                {
                    byte r, g, b;
                    BitmapHelper.Convert565ToRGB((byte)low, (byte)high, out r, out g, out b);
                    HsvHelper.ColorToHSL(r, g, b, out h, out s, out v);
                    map[low + high * 256, 0] = h;
                    map[low + high * 256, 1] = s;
                    map[low + high * 256, 2] = v;
                }
            }

            _log.DebugElapsed("CreateMap");

            return map;
        }

        //public static Color ColorFromHSV(double hue, double saturation, double value)
        //{
        //    int hi = Convert.ToInt32(Math.Floor(hue / 60)) % 6;
        //    double f = hue / 60 - Math.Floor(hue / 60);

        //    value = value * 255;
        //    int v = Convert.ToInt32(value);
        //    int p = Convert.ToInt32(value * (1 - saturation));
        //    int q = Convert.ToInt32(value * (1 - f * saturation));
        //    int t = Convert.ToInt32(value * (1 - (1 - f) * saturation));

        //    if (hi == 0)
        //        return Color.FromArgb(v, t, p);
        //    else if (hi == 1)
        //        return Color.FromArgb(q, v, p);
        //    else if (hi == 2)
        //        return Color.FromArgb(p, v, t);
        //    else if (hi == 3)
        //        return Color.FromArgb(p, q, v);
        //    else if (hi == 4)
        //        return Color.FromArgb(t, p, v);
        //    else
        //        return Color.FromArgb(v, p, q);
        //}
    }
}
